#include "ws_server.h"
#include "thread_pool.h"
#include "elog.h"
#include "elog_file.h"
#include "jpg_server.h"
#include "motor_controller.h"
#include "rgb_power.h"
#include "timer_manager.h"
#include "track.h"
#include "dwarf_http_album.h"
#include "album.h"
#include "http_server.h"
#include "cam_wide.h"
#include "astro_coordinates/ephemeris.h"
#include "ws_constant.h"
#include "sky_seacher/sky_seger.h"
#include "sky_seacher/sky_seacher.h"
#include "dwarf2_panorama.h"
#include "system.h"
#include "notify.h"
#include "dwarf_utils.h"
#include "state_manager.h"
#include "iTips.h"
#include "client/linux/handler/exception_handler.h"

#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>

#include <thread>
#include <functional>
#include <filesystem>


static TimerManager restart_timer_manager;

void init_track(Track *track) {
    track->initTrack();
}

void back_ground_task() {
    Album album;

    if (checkSdExist() < 0) {   
        log_e("check tfcard not exit");
        return;
    } else {
        album.init();
    }
}

int init_elog() {
    //日志目录
    if (access("/userdata/log/dwarf/", F_OK) < 0) {
        system("mkdir -p /userdata/log/dwarf/");
    }
    /* close printf buffer */
    setbuf(stdout, NULL);
    /* initialize EasyLogger */
    elog_init();
    elog_file_init();
    /* set EasyLogger log format */
    elog_set_fmt(ELOG_LVL_ASSERT, ELOG_FMT_ALL);
    elog_set_fmt(ELOG_LVL_ERROR, ELOG_FMT_LVL | ELOG_FMT_TIME | ELOG_FMT_FUNC);
    elog_set_fmt(ELOG_LVL_WARN, ELOG_FMT_LVL | ELOG_FMT_TIME | ELOG_FMT_FUNC);
    elog_set_fmt(ELOG_LVL_INFO, ELOG_FMT_LVL | ELOG_FMT_TIME | ELOG_FMT_FUNC);
    elog_set_fmt(ELOG_LVL_DEBUG, ELOG_FMT_ALL & ~ELOG_FMT_FUNC);
    elog_set_fmt(ELOG_LVL_VERBOSE, ELOG_FMT_ALL & ~ELOG_FMT_FUNC);

    /* start EasyLogger */
    elog_start();

    return 0;
}

void testMotor(StepMotorUser& motor_test) {
    double angle = 20.0;
    double speed = 20.0;
    int pulse = 900;
    int frequency = 300;

    // 20度/s，32细分，1000步加减速，左转20度
    int ret = motor_test.setParameterInDegree(angle, speed, STEP_MOTOR_MINISTEP_32, STEP_MOTOR_RAMP_PULSE_1000, YAW_LEFT);
    if (ret < 0)
        return;

    motor_test.run();

    if(motor_test.waitMotorToStop() < 0)
        log_i("waitMotorToStop OVERTIME");

    // 300Hz，256细分，100步加减速，追星模式，右转900脉冲
    ret = motor_test.setParameterInPulse(pulse, frequency, STEP_MOTOR_MINISTEP_256, STEP_MOTOR_RAMP_PULSE_100, YAW_RIGHT, STEP_MOTOR_HIRES_MODE_ON);
    if (ret < 0)
        return;

    motor_test.run();
    
    if(motor_test.waitMotorToStop() < 0)
        log_i("waitMotorToStop OVERTIME");

    // 20度/s，256细分，100步加减速，连续模式，左转
    ret = motor_test.setParameterInDegree(angle, speed, STEP_MOTOR_MINISTEP_256, STEP_MOTOR_RAMP_PULSE_100, YAW_LEFT);
    if (ret < 0)
        return;

    motor_test.run();

    // 转1s
    usleep(1000 * 1000);

    // 变速10度/s，右转
    speed = 10.0;
    motor_test.changeSpeed(speed);
    motor_test.changeDirection(YAW_RIGHT);

    // 转1s
    usleep(1000 * 1000);

    // 停止
    motor_test.stop();

    // 变方向失败
    motor_test.changeDirection(YAW_RIGHT);

    // 向右复位，并回到复位前的位置
    motor_test.resetAndRunInDegreeInParallel(YAW_RIGHT, 15);

    // 等待电机运行
    usleep(100 * 1000);

    // 读取相对位置
    int relative_position = 0;
    motor_test.getRelativePosition(relative_position);

    while(relative_position != 0) {
        motor_test.getRelativePosition(relative_position);
        log_i("relative_position:%d", relative_position);
        usleep(10 * 1000);
    }

    // 等待电机执行完毕
    motor_test.parallelReturn();

    // 读取1细分下的绝对位置
    int absolute_position = 0;
    ret = motor_test.getAbsolutePositionInPulse(STEP_MOTOR_MINISTEP_1, absolute_position);
    if (ret < 0)
        return;

    // 到达1细分，50的位置
    ret = motor_test.setParameterInPositionInPulse(50, MotorController::motor_yaw_max_frequency_, STEP_MOTOR_MINISTEP_1, STEP_MOTOR_RAMP_PULSE_0);
    if (ret < 0)
        return;

    motor_test.run();

    if(motor_test.waitMotorToStop() < 0)
        log_i("waitMotorToStop OVERTIME");

    // 读取1细分下的绝对位置
    motor_test.getAbsolutePositionInPulse(STEP_MOTOR_MINISTEP_1, absolute_position);

    // 到达最大软限位处
    ret = motor_test.setParameterInPositionInDegree(MotorController::motor_yaw_max_soft_limit_, MotorController::motor_yaw_max_frequency_, 
                                                    STEP_MOTOR_MINISTEP_4, STEP_MOTOR_RAMP_PULSE_0);
    if (ret < 0)
        return;

    motor_test.run();

    if(motor_test.waitMotorToStop() < 0)
        log_i("waitMotorToStop OVERTIME");

    // 读取1细分下的绝对位置
    motor_test.getAbsolutePositionInPulse(STEP_MOTOR_MINISTEP_1, absolute_position);
}

void camWideTestforSkyAndGround() {
    const double exp = 1.0;
    const int gain = 164;

    CamWide cam_wide;
    std::thread cam_wide_open([&]() {
        cam_wide.openCamera(0); 
    });
    cam_wide_open.detach();
    usleep(1 * 1000);// 等待打开摄像头上锁
    cam_wide.setExpMode(1);
    cam_wide.setExp(exp);
    cam_wide.setGain(gain);

    // 半个广角视野
    const double img_yaw_angle = 18;
    const double img_pitch_angle = 12;

    StepMotorUser& motor_yaw = MotorController::motorYaw();
    StepMotorUser& motor_pitch = MotorController::motorPitch();

    motor_yaw.resetAndRunInDegreeInParallel();
    motor_pitch.resetAndRunInPulseInParallel();
    motor_yaw.parallelReturn();
    motor_pitch.parallelReturn();

    double alt = 0;
    motor_pitch.getAbsolutePositionInDegree(STEP_MOTOR_MINISTEP_8, alt);
    alt -= 90;
    double yaw_angle = RADIANS_TO_DEGREES(acos((COSD(img_yaw_angle) - SIND(alt) * SIND(alt)) / (COSD(alt) * COSD(alt))));

    bool direction = YAW_RIGHT;

    while(1) {
        cam_wide.photograph();

        motor_yaw.setParameterInDegree(yaw_angle, 32.0, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_1000, direction);
        motor_yaw.run();
        int ret = motor_yaw.waitMotorToStop();

        if (ret == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED) {
            direction = !direction;

            motor_pitch.setParameterInDegree(img_pitch_angle, 32.0, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_1000, PITCH_UP);
            motor_pitch.run();
            motor_pitch.waitMotorToStop();

            motor_pitch.getAbsolutePositionInDegree(STEP_MOTOR_MINISTEP_8, alt);
            alt -= 90;
            if (alt >= 90) 
                return;
            yaw_angle = RADIANS_TO_DEGREES(acos((COSD(img_yaw_angle) - SIND(alt) * SIND(alt)) / (COSD(alt) * COSD(alt))));
        }

        sleep(exp * 2);
    }
}

static bool filterCallback(void* context) {
    system("rm /userdata/log/dwarf/*.dmp");
    return true;
}

static bool dumpCallback(const google_breakpad::MinidumpDescriptor& descriptor, void* context, bool succeeded) {
    log_i("Dump path: %s\n", descriptor.path());
    return succeeded;
}

int main() {
    init_elog();

    // system("date -s \"2038-01-01 00:00:00 UTC\"");

    google_breakpad::MinidumpDescriptor descriptor("/userdata/log/dwarf");
    google_breakpad::ExceptionHandler eh(descriptor, filterCallback, dumpCallback, NULL, true, -1);

    ThreadPool* thread_pool = ThreadPool::GetInstance(50);
    
    MotorController::initMotorController();
    StateManager* state_manager = StateManager::GetInstance();
    // init rkmedia in main  thread
    state_manager->cam_tele_.initRKmedia();

    // pid_t child_pid = fork();

    // if (child_pid == 0) {
    //     // 子进程代码
    //     execl("/etc/init.d/S50nginx", "S50nginx", "restart", NULL);                   
    //     _exit(1); // 在子进程中执行失败后退出
    // } else if (child_pid < 0) {
    //     perror("fork");
    //     return 1;
    // } else {
    //     // 等待子进程结束
    //     int status;
    //     waitpid(child_pid, &status, 0);

    //     if (WIFEXITED(status)) {
    //         // 子进程正常退出，可以获取退出状态
    //         int exit_status = WEXITSTATUS(status);
    //     }
    // }

    startServer();

    //电机初始化
    MotorController::initMotorController();

    //装载天空分割模型
    // SkySeger::loadModel();

    // std::vector<HorizontalCoordinates> calibration_path;
    // SkySeacher().start(calibration_path);

    //广角拍天空
    // thread cam_wide_test(camWideTestforSkyAndGround);
    // cam_wide_test.detach();

    // //测试智能感叹号
    // std::thread itips_thread([&]() {
    //     while (1) {
    //         ITips::itips().getAstroITips();
    //         sleep(1);
    //     }
    // });
    // itips_thread.detach();

    //串口初始化
    RgbPower rgb_power;
    rgb_power.initRgbPower();

    // 跟踪初始
    Track track;
    thread init_track_t(init_track, &track);
    init_track_t.detach();

    // // 后台任务
    thread back_ground_t(back_ground_task);
    back_ground_t.detach();

    System m_system;
    TimerManager timer_manager;
    m_system.getSdCardInfo(0);
    timer_manager.startTimer([&m_system](int elapsed_time) {
        m_system.getSdCardInfo(elapsed_time);
    }, 60);

    TimerManager timer_manager_check_ele;
    timer_manager_check_ele.startTimer([state_manager](int elapsed_time) {
        state_manager->checkEle();
    }, 30);

    TimerManager timer_manager_close_camera;
    timer_manager_close_camera.startTimer([state_manager](int elapsed_time) {
        state_manager->checkCloseCamera();
    }, 30);

    // Notify* notify = Notify::getInstance();
    // TimerManager timer_manager_notify;
    // timer_manager_notify.startTimer([notify](int elapsed_time) {
    //     notify->pollWsMessage();
    // }, 5);

    dwarf::HttpServer http_server;  
    http_server.startServer();

    WsServer ws_server;
    ws_server.startWsServer();

    return 0;
}